#include "Transform.hpp"

/**
 * @brief  旋转矩阵和平移向量合并成齐次矩阵
 * @param  R       旋转矩阵         
 * @param  T       平移向量        
 * @return Mat     齐次矩阵         
 */
Mat RTMergeToH(const Mat &R, const Mat &T)
{
    Mat H;
    Mat_<double> R1 = (Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
                       R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
                       R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
                       0, 0, 0);
    Mat_<double> T1 = (Mat_<double>(4, 1) << T.at<double>(0, 0),
                       T.at<double>(1, 0),
                       T.at<double>(2, 0),
                       1);
    cv::hconcat(R1, T1, H); //矩阵拼接
    return H;
}

/**
 * @brief  齐次矩阵拆分成旋转矩阵和平移向量
 * @param  H        齐次矩阵        
 * @param  R        旋转矩阵        
 * @param  T        平移向量        
 */
void HSplitToRT(Mat &H, Mat &R, Mat &T)
{
    Rect R_rect(0, 0, 3, 3);
    Rect T_rect(3, 0, 1, 3);
    R = H(R_rect);
    T = H(T_rect);
}

/**
 * @brief  旋转矩阵的转置矩阵是它的逆矩阵
 *             逆矩阵 * 矩阵 = 单位矩阵
 * @param  R           旋转矩阵 
 * @return bool              
 */
bool isRotatedMatrix(Mat &R)
{
    //无论输入是几阶矩阵，均提取它的三阶矩阵
    Mat temp33 = R({0, 0, 3, 3});
    Mat Rt;
    //转置矩阵
    transpose(temp33, Rt);
    //是旋转矩阵则乘积为单位矩阵
    Mat shouldBeIdentity = Rt * temp33;
    Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

    return cv::norm(I, shouldBeIdentity) < 1e-6;
}

/**
 * @brief   将输入的四元数转换成旋转矩阵
 * @param  q  四元数              
 * @return Mat   旋转矩阵   
 */
Mat QToRMat(const Vec4d &q)
{
    double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];

    double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
    double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
    double q1q2 = q1 * q2, q1q3 = q1 * q3;
    double q2q3 = q2 * q3;
    // 四元数计算公式
    Mat RotMtr = (Mat_<double>(3, 3) << (q0q0 + q1q1 - q2q2 - q3q3), 2 * (q1q2 + q0q3), 2 * (q1q3 - q0q2),
                  2 * (q1q2 - q0q3), (q0q0 - q1q1 + q2q2 - q3q3), 2 * (q2q3 + q0q1),
                  2 * (q1q3 + q0q2), 2 * (q2q3 - q0q1), (q0q0 - q1q1 - q2q2 + q3q3));
    return RotMtr;
}

/**
 * @brief  欧拉角转成旋转矩阵
 * @param  euler     欧拉角
 * @param  seq       坐标轴旋转顺序    
 * @return Mat       旋转矩阵         
 */
Mat eulerToRMat(const Mat &euler, const std::string &seq)
{
    CV_Assert(euler.rows == 1 && euler.cols == 3); //检查参数是否正确

    euler /= (180 / CV_PI); //度转弧度

    Matx13d m(euler); //<double, 1, 3>

    auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
    auto rxs = sin(rx), rxc = cos(rx);
    auto rys = sin(ry), ryc = cos(ry);
    auto rzs = sin(rz), rzc = cos(rz);

    //XYZ方向的旋转矩阵
    Mat RotX = (Mat_<double>(3, 3) << 1, 0, 0,
                0, rxc, -rxs,
                0, rxs, rxc);
    Mat RotY = (Mat_<double>(3, 3) << ryc, 0, rys,
                0, 1, 0,
                -rys, 0, ryc);
    Mat RotZ = (Mat_<double>(3, 3) << rzc, -rzs, 0,
                rzs, rzc, 0,
                0, 0, 1);
    //按顺序合成后的旋转矩阵
    cv::Mat rotMat;

    if (seq == "zyx")
        rotMat = RotX * RotY * RotZ;
    else if (seq == "yzx")
        rotMat = RotX * RotZ * RotY;
    else if (seq == "zxy")
        rotMat = RotY * RotX * RotZ;
    else if (seq == "yxz")
        rotMat = RotZ * RotX * RotY;
    else if (seq == "xyz")
        rotMat = RotZ * RotY * RotX;
    else if (seq == "xzy")
        rotMat = RotY * RotZ * RotX;
    else
    {
        cout << "Euler Angle Sequence string is wrong...";
    }
    //欧拉角特殊情况下会出现死锁
    if (!isRotatedMatrix(rotMat))
    {
        cout << "Euler Angle convert to RotatedMatrix failed..." << endl;
        exit(-1);
    }
    return rotMat;
}

/**
 * @brief  笛卡尔位姿坐标转换成齐次变换矩阵
 * @param  Carte             位姿坐标  x,y,z,rx,ry,rz  or x,y,z, q0,q1,q2,q3,rx,ry,rz  
 * @param  useQuaternion    是否使用四元数
 * @param  seq              坐标轴旋转顺序  
 * @return Mat              
 */
Mat CarteToHMat(const Mat &Carte, bool useQuaternion, const string &seq)
{
    CV_Assert(Carte.total() == 6 || Carte.total() == 10);

    Mat temp = Mat::eye(4, 4, CV_64FC1);

    if (useQuaternion)
    {
        //读取存储的四元数
        Vec4d quaternionVec = Carte({3, 0, 4, 1});
        QToRMat(quaternionVec).copyTo(temp({0, 0, 3, 3}));
    }
    else
    {
        Mat rotVec;
        if (Carte.total() == 6)
        {
            //读取存储的欧拉角
            rotVec = Carte({3, 0, 3, 1});
        }
        if (Carte.total() == 10)
        {
            rotVec = Carte({7, 0, 3, 1});
        }
        //如果seq为空，表示传入的是3*1旋转向量，否则，传入的是欧拉角
        if (0 == seq.compare(""))
        {
            // 罗德里斯转换
            Rodrigues(rotVec, temp({0, 0, 3, 3}));
        }
        else
        {
            eulerToRMat(rotVec, seq).copyTo(temp({0, 0, 3, 3}));
        }
    }
    //存入平移矩阵
    temp({3, 0, 1, 3}) = Carte({0, 0, 3, 1}).t();
    //返回转换结束的齐次矩阵
    return temp;
}
